﻿#include "CamCalCenter.h"
#include <wy.h>
#include <QBuffer>
#include <QDebug>
#include <QFile>
#include <QImage>
#include <QSharedMemory>
#include <eigen3/Eigen/Dense>
#include "JsonUtil.h"
using namespace Eigen;

static Matrix<float, 3, 3> ROT1, ROT2, ROT3, ROT4;
static Vector3f D1, D2, D3, DR1, DR2, DR3;

//----------------------------------------------------
// 三台realsense配置
static Vector3f DR[3] = {DR1, DR2, DR3};
static Vector3f D[3] = {D1, D2, D3};
static Matrix<float, 3, 3> ROTs[3] = {ROT1, ROT2, ROT3};

static float camParam[4][7] = {{0}};
static float armParam[2][7] = {{0}};

static void armParam_F()
{
    QFile f("armParam.csv");
    f.open(QIODevice::ReadOnly);
    QString strF(f.readAll());
    QStringList lsF = strF.split("\n", Qt::SkipEmptyParts);
    int row = -1;
    for (auto s : lsF) {
        row += 1;
        int col = -1;
        QStringList ss = s.split(",", Qt::SkipEmptyParts);
        for (auto x : ss) {
            ++col;
            armParam[row][col] = x.toFloat();
        }
    }
}

static void camParam_F()
{
    QFile f("camParam.csv");
    f.open(QIODevice::ReadOnly);
    QString strF(f.readAll());
    QStringList lsF = strF.split("\n", Qt::SkipEmptyParts);
    int row = -1;
    for (auto s : lsF) {
        row += 1;
        int col = -1;
        QStringList ss = s.split(",", Qt::SkipEmptyParts);
        for (auto x : ss) {
            ++col;
            camParam[row][col] = x.toFloat();
        }
    }
}

static void camPos()
{
    camParam_F();
    // realsense安装角度旋转
    float rx1 = camParam[0][0];
    float ry1 = camParam[0][1];
    float rz1 = camParam[0][2];
    ROT1 << cos(ry1) * cos(rz1),
        cos(rz1) * sin(rx1) * sin(ry1) - cos(rx1) * sin(rz1),
        cos(rx1) * cos(rz1) * sin(ry1) + sin(rx1) * sin(rz1),
        cos(ry1) * sin(rz1),
        cos(rx1) * cos(rz1) + sin(rx1) * sin(ry1) * sin(rz1),
        cos(rx1) * sin(ry1) * sin(rz1) - cos(rz1) * sin(rx1), -sin(ry1),
        cos(ry1) * sin(rx1), cos(rx1) * cos(ry1);
    // realsense安装位置平移
    D1.x() = camParam[0][3];
    D1.y() = camParam[0][4];
    D1.z() = camParam[0][5];
    DR1.x() = camParam[0][6];
    DR1.y() = 0;
    DR1.z() = 0;

    float rx2 = camParam[1][0];
    float ry2 = camParam[1][1];
    float rz2 = camParam[1][2];
    ROT2 << cos(ry2) * cos(rz2),
        cos(rz2) * sin(rx2) * sin(ry2) - cos(rx2) * sin(rz2),
        cos(rx2) * cos(rz2) * sin(ry2) + sin(rx2) * sin(rz2),
        cos(ry2) * sin(rz2),
        cos(rx2) * cos(rz2) + sin(rx2) * sin(ry2) * sin(rz2),
        cos(rx2) * sin(ry2) * sin(rz2) - cos(rz2) * sin(rx2), -sin(ry2),
        cos(ry2) * sin(rx2), cos(rx2) * cos(ry2);
    D2.x() = camParam[1][3];
    D2.y() = camParam[1][4];
    D2.z() = camParam[1][5];
    DR2.x() = camParam[1][6];
    DR2.y() = 0;
    DR2.z() = 0;

    float rx3 = camParam[2][0];
    float ry3 = camParam[2][1];
    float rz3 = camParam[2][2];
    ROT3 << cos(ry3) * cos(rz3),
        cos(rz3) * sin(rx3) * sin(ry3) - cos(rx3) * sin(rz3),
        cos(rx3) * cos(rz3) * sin(ry3) + sin(rx3) * sin(rz3),
        cos(ry3) * sin(rz3),
        cos(rx3) * cos(rz3) + sin(rx3) * sin(ry3) * sin(rz3),
        cos(rx3) * sin(ry3) * sin(rz3) - cos(rz3) * sin(rx3), -sin(ry3),
        cos(ry3) * sin(rx3), cos(rx3) * cos(ry3);
    D3.x() = camParam[2][3];
    D3.y() = camParam[2][4];
    D3.z() = camParam[2][5];
    DR3.x() = camParam[2][6];
    DR3.y() = 0;
    DR3.z() = 0;

    float rx4 = camParam[3][0];
    float ry4 = camParam[3][1];
    float rz4 = camParam[3][2];
    ROT4 << cos(ry4) * cos(rz4),
        cos(rz4) * sin(rx4) * sin(ry4) - cos(rx4) * sin(rz4),
        cos(rx4) * cos(rz4) * sin(ry4) + sin(rx4) * sin(rz4),
        cos(ry4) * sin(rz4),
        cos(rx4) * cos(rz4) + sin(rx4) * sin(ry4) * sin(rz4),
        cos(rx4) * sin(ry4) * sin(rz4) - cos(rz4) * sin(rx4), -sin(ry4),
        cos(ry4) * sin(rx4), cos(rx4) * cos(ry4);

    ROTs[0] = ROT1;
    ROTs[1] = ROT2;
    ROTs[2] = ROT3;

    D[0] = D1;
    D[1] = D2;
    D[2] = D3;

    DR[0] = DR1;
    DR[1] = DR2;
    DR[2] = DR3;
}

CamCalCenter::CamCalCenter(int argc, char** argv, QObject* p)
    : argc_(argc), argv_(argv), QObject(p), m_bRunRealsense(false)
{
    m_ros_spin_thread =
        std::thread{std::bind(&CamCalCenter::rosSpinThread, this)};

    // 所有共享内存都在这里新建
    m_smImg = new QSharedMemory("sm_img");
    bool b = m_smImg->create(921600 * 3);  // rgb*3
    qDebug() << "create sm_img: " << b;

    m_smDepth = new QSharedMemory("sm_depth");
    b = m_smDepth->create(640 * 480 * 4 * 3 * 3);  // depth*3
    qDebug() << "create sm_depth: " << b;

    m_smBaSend =
        new QSharedMemory("sm_basend");  // 用于发送处理好的depth+img ba

    b = m_smBaSend->create(640 * 480 * 4 * 3 * 3 +
                           1024 * 1024 * 4);  // depth*3 + 4M(for jpg)
    qDebug() << "create sm_basend: " << b;

    m_smStatus = new QSharedMemory("sm_status");
    // if (!m_smStatus->isAttached())
    //{
    //	m_smStatus->attach();
    // }
    b = m_smStatus->create(sizeof(SM_Status));
    qDebug() << "create sm_status: " << b;

    m_threadProcessPc =
        std::thread{std::bind(&CamCalCenter::processPc_proc, this)};

    camPos();
    armParam_F();
}

CamCalCenter::~CamCalCenter()
{
    qDebug() << __FUNCTION__ << " called";
    rclcpp::shutdown();
}

void CamCalCenter::rosSpinThread()
{
    rclcpp::init(argc_, argv_);
    m_robotCmdNode = std::make_shared<RobotCmdNode>(this);
    rclcpp::spin(m_robotCmdNode);
    rclcpp::shutdown();
}

// target info
static bool s_bSendRsPos = false;
static QString s_strSerialSendRsPos = "";
static float s_fTargetXL = 0.8000f;
static float s_fTargetYL = -0.0400f;
static float s_fTargetZL = 0.0700f;
static float s_fTargetXR = 0.7200f;
static float s_fTargetYR = -0.2100f;
static float s_fTargetZR = 0.2600f;
static float s_fTargetXwL = 0.0400f;
static float s_fTargetYwL = 0.0600f;
static float s_fTargetZwL = 0.2600f;
static float s_fTargetXwR = 0.1000f;
static float s_fTargetYwR = 0.2600f;
static float s_fTargetZwR = 0.1000f;
static int s_nMissionNum = -1;
static bool s_bStartGenR = false;
static QVariantList s_listSteps;

static int s_nCurStepIndex = 0;

static int stepNum[1] = {0};
static float TPS_data[10][22] = {{0}};
static float toolmass_L_data[10][4] = {{0}};
static float toolmass_R_data[10][4] = {{0}};
static float cc_L_data[10][12] = {{0}};
static float cc_R_data[10][12] = {{0}};
static float ftMode_L_data[10] = {0};
static float ftMode_R_data[10] = {0};
static float period_L[10][2] = {{0}};
static float period_R[10][2] = {{0}};

static double DL_tg[6] = {0};
static quint16 DL_state = 0;
static double endStateRTL[6] = {0};
static double endStateRTR[6] = {0};
static float tgRS[30] = {0};
static int lineget_flag_RSL = 0;
static int lineget_flag_RSR = 0;
static int rowReady = 0;

static float* BoxPts_x_L = new float[640 * 480];
static float* BoxPts_y_L = new float[640 * 480];
static float* BoxPts_z_L = new float[640 * 480];
static float* BoxPts_x_R = new float[640 * 480];
static float* BoxPts_y_R = new float[640 * 480];
static float* BoxPts_z_R = new float[640 * 480];
static float* m_bufPts = new float[640 * 480 * 3 * 3];

static QVariantList RS_tg;

void CamCalCenter::onTgRsReady()
{
    QVariantMap tg_rs = RS_tg.at(0).toMap();

    QVariantMap RS_tg_data = tg_rs.value("RS_tg_data").toMap();
    QVariantMap RS_tg_state = tg_rs.value("RS_tg_state").toMap();

    tgRS[0] = RS_tg_data.value("x_re2_z_L_m").toFloat();
    tgRS[1] = RS_tg_data.value("y_re2_z_L_m").toFloat();
    tgRS[2] = RS_tg_data.value("z_re2_z_L_m").toFloat();
    tgRS[3] = RS_tg_data.value("rx_re2_z_L_m").toFloat();
    tgRS[4] = RS_tg_data.value("ry_re2_z_L_m").toFloat();
    tgRS[5] = RS_tg_data.value("rz_re2_z_L_m").toFloat();

    tgRS[6] = RS_tg_data.value("x_re2_y_L_m").toFloat();
    tgRS[7] = RS_tg_data.value("y_re2_y_L_m").toFloat();
    tgRS[8] = RS_tg_data.value("z_re2_y_L_m").toFloat();
    tgRS[9] = RS_tg_data.value("rx_re2_y_L_m").toFloat();
    tgRS[10] = RS_tg_data.value("ry_re2_y_L_m").toFloat();
    tgRS[11] = RS_tg_data.value("rz_re2_y_L_m").toFloat();

    tgRS[12] = RS_tg_data.value("x_re2_z_R_m").toFloat();
    tgRS[13] = RS_tg_data.value("y_re2_z_R_m").toFloat();
    tgRS[14] = RS_tg_data.value("z_re2_z_R_m").toFloat();
    tgRS[15] = RS_tg_data.value("rx_re2_z_R_m").toFloat();
    tgRS[16] = RS_tg_data.value("ry_re2_z_R_m").toFloat();
    tgRS[17] = RS_tg_data.value("rz_re2_z_R_m").toFloat();

    tgRS[18] = RS_tg_data.value("x_re2_y_R_m").toFloat();
    tgRS[19] = RS_tg_data.value("y_re2_y_R_m").toFloat();
    tgRS[20] = RS_tg_data.value("z_re2_y_R_m").toFloat();
    tgRS[21] = RS_tg_data.value("rx_re2_y_R_m").toFloat();
    tgRS[22] = RS_tg_data.value("ry_re2_y_R_m").toFloat();
    tgRS[23] = RS_tg_data.value("rz_re2_y_R_m").toFloat();

    lineget_flag_RSL = RS_tg_state.value("lineget_flag_L").toInt();
    lineget_flag_RSR = RS_tg_state.value("lineget_flag_R").toInt();

    // for (int i = 0; i < 24; i++)
    //{
    //	qDebug() << "tgRS[" << i << "] = " << tgRS[i];
    // }

    memcpy(&m_dataSMStatus, tgRS, sizeof(float) * 24);
    m_dataSMStatus.lineget_flag_RSL = lineget_flag_RSL;
    m_dataSMStatus.lineget_flag_RSR = lineget_flag_RSR;

    m_smStatus->lock();
    memcpy((char*)m_smStatus->data(), &m_dataSMStatus, sizeof(SM_Status));
    m_smStatus->unlock();
}

void CamCalCenter::processPc_proc()
{
    while (true) {
        std::this_thread::sleep_for(std::chrono::milliseconds(809));
        if (!m_bRunRealsense) {
            continue;
        }

        processPc();
    }
}

//-------------------------------------------------------
// 均值窗口内数组
static float p_re2_z_L_s[10][3] = {{0}};
static float r_re2_z_L_s[10][3] = {{0}};
static float p_re2_y_L_s[10][3] = {{0}};
static float r_re2_y_L_s[10][3] = {{0}};

static float p_re2_z_R_s[10][3] = {{0}};
static float r_re2_z_R_s[10][3] = {{0}};
static float p_re2_y_R_s[10][3] = {{0}};
static float r_re2_y_R_s[10][3] = {{0}};

// 均值滤波器窗口索引
static int ROW = 0;
//------------------------
// 目标框保存，判断是否移动
static float xmaxL_old = 0;
static float xminL_old = 0;
static float ymaxL_old = 0;
static float yminL_old = 0;
static float zmaxL_old = 0;
static float zminL_old = 0;

static float xmaxR_old = 0;
static float xminR_old = 0;
static float ymaxR_old = 0;
static float yminR_old = 0;
static float zmaxR_old = 0;
static float zminR_old = 0;

void CamCalCenter::processPc()
{
    // 拟合窗位置和尺寸计算
    float xmaxL = s_fTargetXL + s_fTargetXwL / 2;
    float xminL = s_fTargetXL - s_fTargetXwL / 2;
    float ymaxL = s_fTargetYL + s_fTargetYwL / 2;
    float yminL = s_fTargetYL - s_fTargetYwL / 2;
    float zmaxL = s_fTargetZL + s_fTargetZwL / 2;
    float zminL = s_fTargetZL - s_fTargetZwL / 2;

    float xmaxR = s_fTargetXR + s_fTargetXwR / 2;
    float xminR = s_fTargetXR - s_fTargetXwR / 2;
    float ymaxR = s_fTargetYR + s_fTargetYwR / 2;
    float yminR = s_fTargetYR - s_fTargetYwR / 2;
    float zmaxR = s_fTargetZR + s_fTargetZwR / 2;
    float zminR = s_fTargetZR - s_fTargetZwR / 2;

    bool inRangeL = (s_fTargetXL > 0.3) && (s_fTargetXL < 0.8) && (s_fTargetYL > -0.3) && (s_fTargetYL < 0.3) && (s_fTargetZL > -0.1) && (s_fTargetZL < 1) && (s_fTargetYL > s_fTargetYR);
    bool inRangeR = (s_fTargetXR > 0.3) && (s_fTargetXR < 0.8) && (s_fTargetYR > -0.3) && (s_fTargetYR < 0.3) && (s_fTargetZR > -0.1) && (s_fTargetZR < 1) && (s_fTargetYL > s_fTargetYR);

    float a = D2[2];
    float b = 0.09f;
    float d2 = D2[1];

    float cx2 = 325.101f;
    float cy2 = 234.98f;
    float fx2 = 609.092f;
    float fy2 = 609.249f;

    float ymin_rgbL = 640 - ((yminL - d2) * fx2 / (s_fTargetXL - b) + cx2);
    float ymax_rgbL = 640 - ((ymaxL - d2) * fx2 / (s_fTargetXL - b) + cx2);
    float zmin_rgbL = 480 + 480 - ((zminL - a) * fy2 / (s_fTargetXL - b) + cy2);
    float zmax_rgbL = 480 + 480 - ((zmaxL - a) * fy2 / (s_fTargetXL - b) + cy2);
    float ytarget_rgbL = 640 - ((s_fTargetYL - d2) * fx2 / (s_fTargetXL - b) + cx2);
    float ztarget_rgbL = 480 + 480 - ((s_fTargetZL - a) * fy2 / (s_fTargetXL - b) + cy2);

    float ymin_rgbR = 640 - ((yminR - d2) * fx2 / (s_fTargetXR - b) + cx2);
    float ymax_rgbR = 640 - ((ymaxR - d2) * fx2 / (s_fTargetXR - b) + cx2);
    float zmin_rgbR = 480 + 480 - ((zminR - a) * fy2 / (s_fTargetXR - b) + cy2);
    float zmax_rgbR = 480 + 480 - ((zmaxR - a) * fy2 / (s_fTargetXR - b) + cy2);
    float ytarget_rgbR = 640 - ((s_fTargetYR - d2) * fx2 / (s_fTargetXR - b) + cx2);
    float ztarget_rgbR = 480 + 480 - ((s_fTargetZR - a) * fy2 / (s_fTargetXR - b) + cy2);
    //--------------------
    bool unChange1 = (xmaxL_old == xmaxL) && (xminL_old == xminL) &&
                     (ymaxL_old == ymaxL) && (yminL_old == yminL) &&
                     (zmaxL_old == zmaxL) && (zminL_old == zminL);
    bool unChange2 = (xmaxR_old == xmaxR) && (xminR_old == xminR) &&
                     (ymaxR_old == ymaxR) && (yminR_old == yminR) &&
                     (zmaxR_old == zmaxR) && (zminR_old == zminR);

    xmaxL_old = xmaxL;
    xminL_old = xminL;
    ymaxL_old = ymaxL;
    yminL_old = yminL;
    zmaxL_old = zmaxL;
    zminL_old = zminL;

    xmaxR_old = xmaxR;
    xminR_old = xminR;
    ymaxR_old = ymaxR;
    yminR_old = yminR;
    zmaxR_old = zmaxR;
    zminR_old = zminR;
    //----------------------

    float xmin = 999999.0;
    float ymin = 999999.0;
    float zmin = 999999.0;
    float xmax = -999999.0;
    float ymax = -999999.0;
    float zmax = -999999.0;
    int coorCnt = 0;
    //--------------------------------------------------------------------
    int boxptsCnt_L = 0;  // 左手拟合框内点数量
    int boxptsCnt_R = 0;  // 右手拟合框内点数量

    int lineget_flag_L = 0;  // 左手拟合成功标志位
    int lineget_flag_R = 0;  // 左手拟合成功标志位

    float p_re2_z_L[3] = {0, 0, 0}, r_re2_z_L[3] = {0, 0, 0},
          p_re2_y_L[3] = {0, 0, 0},
          r_re2_y_L[3] = {0, 0, 0};  // 左手实时拟合结果
    float p2z_max_L[3] = {0, 0, 0}, p2z_min_L[3] = {0, 0, 0},
          p2y_max_L[3] = {0, 0, 0}, p2y_min_L[3] = {0, 0, 0};  // 左手拟合后画点

    float p_re2_z_R[3] = {0, 0, 0}, r_re2_z_R[3] = {0, 0, 0},
          p_re2_y_R[3] = {0, 0, 0},
          r_re2_y_R[3] = {0, 0, 0};  // 右手实时拟合结果
    float p2z_max_R[3] = {0, 0, 0}, p2z_min_R[3] = {0, 0, 0},
          p2y_max_R[3] = {0, 0, 0}, p2y_min_R[3] = {0, 0, 0};  // 右手拟合后画点

    float p_re2_z_L_m[3] = {0, 0, 0}, r_re2_z_L_m[3] = {0, 0, 0},
          p_re2_y_L_m[3] = {0, 0, 0},
          r_re2_y_L_m[3] = {0, 0, 0};  // 左手拟合均值
    float p_re2_z_R_m[3] = {0, 0, 0}, r_re2_z_R_m[3] = {0, 0, 0},
          p_re2_y_R_m[3] = {0, 0, 0},
          r_re2_y_R_m[3] = {0, 0, 0};  // 右手拟合均值

    // sharememory of depth unlock
    m_smDepth->lock();

    // realsense获取点云并处理
    for (int camIndex = 0; camIndex < 3; ++camIndex) {
        float* vts = (float*)((unsigned char*)m_smDepth->data() +
                              640 * 480 * 4 * 3 * camIndex);

        for (int i = 0; i < 640 * 480 * 3; i += 3) {
            // vts[i+2] 是 z
            // vts[i+1] 是 y
            // vts[i] 是 x
            // 剔除视觉边界以外点
            if (vts[i + 2] > 1.5f || vts[i + 2] < 0.3f || vts[i + 1] > 1.0f ||
                vts[i + 1] < -1.0f || vts[i] > 1.0f || vts[i] < -1.0f) {
                continue;
            }

            Vector3f oneP(vts[i + 2], -vts[i], -vts[i + 1]);
            // qDebug() << "v10: " << vts[i + 2];
            // qDebug() << "v11: " << -vts[i];
            // qDebug() << "v12: " << -vts[i + 1];
            Vector3f v1 = ROTs[camIndex] * (oneP + DR[camIndex]) + D[camIndex];

            //----------------------------------------------------------------------------------
            m_bufPts[coorCnt++] = v1[0];
            m_bufPts[coorCnt++] = v1[1];
            m_bufPts[coorCnt++] = v1[2];

            if (s_bSendRsPos) {
                //---------------------------------------------------------------------------------
                // 保留拟合框内点
                if (v1[0] < xmaxL && v1[0] > xminL && v1[1] < ymaxL &&
                    v1[1] > yminL && v1[2] < zmaxL && v1[2] > zminL) {
                    BoxPts_x_L[boxptsCnt_L] = v1[0];
                    BoxPts_y_L[boxptsCnt_L] = v1[1];
                    BoxPts_z_L[boxptsCnt_L] = v1[2];
                    boxptsCnt_L++;
                }

                if (v1[0] < xmaxR && v1[0] > xminR && v1[1] < ymaxR &&
                    v1[1] > yminR && v1[2] < zmaxR && v1[2] > zminR) {
                    BoxPts_x_R[boxptsCnt_R] = v1[0];
                    BoxPts_y_R[boxptsCnt_R] = v1[1];
                    BoxPts_z_R[boxptsCnt_R] = v1[2];
                    boxptsCnt_R++;
                }
            }
            //---------------------------------------------------------------------------------

            xmax = xmax < v1[0] ? v1[0] : xmax;
            ymax = ymax < v1[1] ? v1[1] : ymax;
            zmax = zmax < v1[2] ? v1[2] : zmax;
            xmin = xmin > v1[0] ? v1[0] : xmin;
            ymin = ymin > v1[1] ? v1[1] : ymin;
            zmin = zmin > v1[2] ? v1[2] : zmin;
        }
    }

    // sharememory of depth unlock
    m_smDepth->unlock();

    //---------------------------------------------------------------------------------
    if (boxptsCnt_L > 10) {
        float p_x2z[2], p_y2z[2], p_x2y[2], p_z2y[2];
        //            float y1 = 0.203f, z1 = 0.165f;

        // 导线拟合
        curvefit(BoxPts_z_L, BoxPts_x_L, boxptsCnt_L, p_x2z);
        curvefit(BoxPts_z_L, BoxPts_y_L, boxptsCnt_L, p_y2z);
        curvefit(BoxPts_y_L, BoxPts_x_L, boxptsCnt_L, p_x2y);
        curvefit(BoxPts_y_L, BoxPts_z_L, boxptsCnt_L, p_z2y);

        // // 导线拟合评估
        // float xRange[2] = {xminL, xmaxL};
        // float yRange[2] = {yminL, ymaxL};
        // float zRange[2] = {zminL, zmaxL};

        // float RR_x2z[1], RR_y2z[1], RR_x2y[1], RR_z2y[1];
        // evaulatecurvefit(BoxPts_z_L, BoxPts_x_L, zRange, xRange, boxptsCnt_L,
        //                  p_x2z, RR_x2z);
        // evaulatecurvefit(BoxPts_z_L, BoxPts_y_L, zRange, yRange, boxptsCnt_L,
        //                  p_y2z, RR_y2z);
        // evaulatecurvefit(BoxPts_y_L, BoxPts_x_L, yRange, xRange, boxptsCnt_L,
        //                  p_x2y, RR_x2y);
        // evaulatecurvefit(BoxPts_y_L, BoxPts_z_L, yRange, zRange, boxptsCnt_L,
        //                  p_z2y, RR_z2y);

        // 计算目标位姿
        p_r_calaculation(p_x2z, p_y2z, p_x2y, p_z2y, s_fTargetYL, s_fTargetZL,
                         p_re2_z_L, r_re2_z_L, p_re2_y_L, r_re2_y_L);

        float zmax_p = *std::max_element(BoxPts_z_L, BoxPts_z_L + boxptsCnt_L);
        float zmin_p = *std::min_element(BoxPts_z_L, BoxPts_z_L + boxptsCnt_L);

        p2z_max_L[0] = p_x2z[0] * zmax_p + p_x2z[1];
        p2z_max_L[1] = p_y2z[0] * zmax_p + p_y2z[1];
        p2z_max_L[2] = zmax_p;

        p2z_min_L[0] = p_x2z[0] * zmin_p + p_x2z[1];
        p2z_min_L[1] = p_y2z[0] * zmin_p + p_y2z[1];
        p2z_min_L[2] = zmin_p;

        float ymax_p = *std::max_element(BoxPts_y_L, BoxPts_y_L + boxptsCnt_L);
        float ymin_p = *std::min_element(BoxPts_y_L, BoxPts_y_L + boxptsCnt_L);

        p2y_max_L[0] = p_x2y[0] * ymax_p + p_x2y[1];
        p2y_max_L[1] = ymax_p;
        p2y_max_L[2] = p_z2y[0] * ymax_p + p_z2y[1];

        p2y_min_L[0] = p_x2y[0] * ymin_p + p_x2y[1];
        p2y_min_L[1] = ymin_p;
        p2y_min_L[2] = p_z2y[0] * ymin_p + p_z2y[1];

        // // 筛选极值点
        // if ((RR_x2z[0] + RR_y2z[0]) < (RR_x2y[0] + RR_z2y[0])) {
        //     float zmax_p =
        //         *std::max_element(BoxPts_z_L, BoxPts_z_L + boxptsCnt_L);
        //     float zmin_p =
        //         *std::min_element(BoxPts_z_L, BoxPts_z_L + boxptsCnt_L);

        //     p2z_max_L[0] = p_x2z[0] * zmax_p + p_x2z[1];
        //     p2z_max_L[1] = p_y2z[0] * zmax_p + p_y2z[1];
        //     p2z_max_L[2] = zmax_p;

        //     p2z_min_L[0] = p_x2z[0] * zmin_p + p_x2z[1];
        //     p2z_min_L[1] = p_y2z[0] * zmin_p + p_y2z[1];
        //     p2z_min_L[2] = zmin_p;

        //     p2y_max_L[0] = 0;
        //     p2y_max_L[1] = 0;
        //     p2y_max_L[2] = 0;

        //     p2y_min_L[0] = 0;
        //     p2y_min_L[1] = 0;
        //     p2y_min_L[2] = 0;
        // } else {
        //     float ymax_p =
        //         *std::max_element(BoxPts_y_L, BoxPts_y_L + boxptsCnt_L);
        //     float ymin_p =
        //         *std::min_element(BoxPts_y_L, BoxPts_y_L + boxptsCnt_L);

        //     p2z_max_L[0] = 0;
        //     p2z_max_L[1] = 0;
        //     p2z_max_L[2] = 0;

        //     p2z_min_L[0] = 0;
        //     p2z_min_L[1] = 0;
        //     p2z_min_L[2] = 0;

        //     p2y_max_L[0] = p_x2y[0] * ymax_p + p_x2y[1];
        //     p2y_max_L[1] = ymax_p;
        //     p2y_max_L[2] = p_z2y[0] * ymax_p + p_z2y[1];

        //     p2y_min_L[0] = p_x2y[0] * ymin_p + p_x2y[1];
        //     p2y_min_L[1] = ymin_p;
        //     p2y_min_L[2] = p_z2y[0] * ymin_p + p_z2y[1];
        // }
        //-------------------------------------------
        // 拟合成功标志位置为1
        lineget_flag_L = 1;
    }

    if (boxptsCnt_R > 10) {
        float p_x2z[2], p_y2z[2], p_x2y[2], p_z2y[2];
        //            float y1 = -0.203f, z1 = 0.165f;

        curvefit(BoxPts_z_R, BoxPts_x_R, boxptsCnt_R, p_x2z);
        curvefit(BoxPts_z_R, BoxPts_y_R, boxptsCnt_R, p_y2z);
        curvefit(BoxPts_y_R, BoxPts_x_R, boxptsCnt_R, p_x2y);
        curvefit(BoxPts_y_R, BoxPts_z_R, boxptsCnt_R, p_z2y);

        // // 导线拟合评估
        // float xRange[2] = {xminR, xmaxR};
        // float yRange[2] = {yminR, ymaxR};
        // float zRange[2] = {zminR, zmaxR};

        // float RR_x2z[1], RR_y2z[1], RR_x2y[1], RR_z2y[1];
        // evaulatecurvefit(BoxPts_z_R, BoxPts_x_R, zRange, xRange, boxptsCnt_L,
        //                  p_x2z, RR_x2z);
        // evaulatecurvefit(BoxPts_z_R, BoxPts_y_R, zRange, yRange, boxptsCnt_L,
        //                  p_y2z, RR_y2z);
        // evaulatecurvefit(BoxPts_y_R, BoxPts_x_R, yRange, xRange, boxptsCnt_L,
        //                  p_x2y, RR_x2y);
        // evaulatecurvefit(BoxPts_y_R, BoxPts_z_R, yRange, zRange, boxptsCnt_L,
        //                  p_z2y, RR_z2y);

        // 计算目标位姿
        p_r_calaculation(p_x2z, p_y2z, p_x2y, p_z2y, s_fTargetYR, s_fTargetZR,
                         p_re2_z_R, r_re2_z_R, p_re2_y_R, r_re2_y_R);

        float zmax_pr = *std::max_element(BoxPts_z_R, BoxPts_z_R + boxptsCnt_R);
        float zmin_pr = *std::min_element(BoxPts_z_R, BoxPts_z_R + boxptsCnt_R);

        p2z_max_R[0] = p_x2z[0] * zmax_pr + p_x2z[1];
        p2z_max_R[1] = p_y2z[0] * zmax_pr + p_y2z[1];
        p2z_max_R[2] = zmax_pr;

        p2z_min_R[0] = p_x2z[0] * zmin_pr + p_x2z[1];
        p2z_min_R[1] = p_y2z[0] * zmin_pr + p_y2z[1];
        p2z_min_R[2] = zmin_pr;

        float ymax_pr = *std::max_element(BoxPts_y_R, BoxPts_y_R + boxptsCnt_R);
        float ymin_pr = *std::min_element(BoxPts_y_R, BoxPts_y_R + boxptsCnt_R);

        p2z_min_R[0] = p_x2z[0] * zmin_pr + p_x2z[1];
        p2z_min_R[1] = p_y2z[0] * zmin_pr + p_y2z[1];
        p2z_min_R[2] = zmin_pr;

        p2y_max_R[0] = p_x2y[0] * ymax_pr + p_x2y[1];
        p2y_max_R[1] = ymax_pr;
        p2y_max_R[2] = p_z2y[0] * ymax_pr + p_z2y[1];

        p2y_min_R[0] = p_x2y[0] * ymin_pr + p_x2y[1];
        p2y_min_R[1] = ymin_pr;
        p2y_min_R[2] = p_z2y[0] * ymin_pr + p_z2y[1];

        // // 筛选极值点
        // if ((RR_x2z[0] + RR_y2z[0]) < (RR_x2y[0] + RR_z2y[0])) {
        //     float zmax_pr =
        //         *std::max_element(BoxPts_z_R, BoxPts_z_R + boxptsCnt_R);
        //     float zmin_pr =
        //         *std::min_element(BoxPts_z_R, BoxPts_z_R + boxptsCnt_R);

        //     p2z_max_R[0] = p_x2z[0] * zmax_pr + p_x2z[1];
        //     p2z_max_R[1] = p_y2z[0] * zmax_pr + p_y2z[1];
        //     p2z_max_R[2] = zmax_pr;

        //     p2z_min_R[0] = p_x2z[0] * zmin_pr + p_x2z[1];
        //     p2z_min_R[1] = p_y2z[0] * zmin_pr + p_y2z[1];
        //     p2z_min_R[2] = zmin_pr;

        //     p2y_max_R[0] = 0;
        //     p2y_max_R[1] = 0;
        //     p2y_max_R[2] = 0;

        //     p2y_min_R[0] = 0;
        //     p2y_min_R[1] = 0;
        //     p2y_min_R[2] = 0;
        // } else {
        //     float ymax_pr =
        //         *std::max_element(BoxPts_y_R, BoxPts_y_R + boxptsCnt_R);
        //     float ymin_pr =
        //         *std::min_element(BoxPts_y_R, BoxPts_y_R + boxptsCnt_R);

        //     p2z_max_R[0] = 0;
        //     p2z_max_R[1] = 0;
        //     p2z_max_R[2] = 0;

        //     p2z_min_R[0] = 0;
        //     p2z_min_R[1] = 0;
        //     p2z_min_R[2] = 0;

        //     p2y_max_R[0] = p_x2y[0] * ymax_pr + p_x2y[1];
        //     p2y_max_R[1] = ymax_pr;
        //     p2y_max_R[2] = p_z2y[0] * ymax_pr + p_z2y[1];

        //     p2y_min_R[0] = p_x2y[0] * ymin_pr + p_x2y[1];
        //     p2y_min_R[1] = ymin_pr;
        //     p2y_min_R[2] = p_z2y[0] * ymin_pr + p_z2y[1];
        // }
        //-------------------------------------------
        lineget_flag_R = 1;
    }

    //-------------------------------------------------------
    // 均值滤波
    // if (ROW == 10)
    if (ROW == 10 || unChange1 == 0 || unChange2 == 0 || inRangeL == 0 ||
        inRangeR == 0) {
        ROW = 0;
    }
    // sigDbgMsg("ROW: "  + QString::number(ROW));

    // 滤波窗填充
    memcpy(&p_re2_z_L_s[ROW][0], &p_re2_z_L[0], sizeof(p_re2_z_L));
    memcpy(&r_re2_z_L_s[ROW][0], &r_re2_z_L[0], sizeof(r_re2_z_L));
    memcpy(&p_re2_y_L_s[ROW][0], &p_re2_y_L[0], sizeof(p_re2_y_L));
    memcpy(&r_re2_y_L_s[ROW][0], &r_re2_y_L[0], sizeof(r_re2_y_L));

    memcpy(&p_re2_z_R_s[ROW][0], &p_re2_z_R[0], sizeof(p_re2_z_R));
    memcpy(&r_re2_z_R_s[ROW][0], &r_re2_z_R[0], sizeof(r_re2_z_R));
    memcpy(&p_re2_y_R_s[ROW][0], &p_re2_y_R[0], sizeof(p_re2_y_R));
    memcpy(&r_re2_y_R_s[ROW][0], &r_re2_y_R[0], sizeof(r_re2_y_R));

    ROW++;

    if (unChange1 == 0 || unChange2 == 0) {
        rowReady = 0;
    }

    // 10周期采样标志位置1
    if (ROW == 10) {
        rowReady = 1;
    }

    float sum1[3] = {0};
    float sum2[3] = {0};
    float sum3[3] = {0};
    float sum4[3] = {0};
    float sum5[3] = {0};
    float sum6[3] = {0};
    float sum7[3] = {0};
    float sum8[3] = {0};

    //-------------------------------------------------------
    // 滤波窗内求和
    for (int i = 0; i < 10; i++) {
        sum1[0] = p_re2_z_L_s[i][0] + sum1[0];
        sum1[1] = p_re2_z_L_s[i][1] + sum1[1];
        sum1[2] = p_re2_z_L_s[i][2] + sum1[2];

        sum2[0] = r_re2_z_L_s[i][0] + sum2[0];
        sum2[1] = r_re2_z_L_s[i][1] + sum2[1];
        sum2[2] = r_re2_z_L_s[i][2] + sum2[2];

        sum3[0] = p_re2_y_L_s[i][0] + sum3[0];
        sum3[1] = p_re2_y_L_s[i][1] + sum3[1];
        sum3[2] = p_re2_y_L_s[i][2] + sum3[2];

        sum4[0] = r_re2_y_L_s[i][0] + sum4[0];
        sum4[1] = r_re2_y_L_s[i][1] + sum4[1];
        sum4[2] = r_re2_y_L_s[i][2] + sum4[2];

        sum5[0] = p_re2_z_R_s[i][0] + sum5[0];
        sum5[1] = p_re2_z_R_s[i][1] + sum5[1];
        sum5[2] = p_re2_z_R_s[i][2] + sum5[2];

        sum6[0] = r_re2_z_R_s[i][0] + sum6[0];
        sum6[1] = r_re2_z_R_s[i][1] + sum6[1];
        sum6[2] = r_re2_z_R_s[i][2] + sum6[2];

        sum7[0] = p_re2_y_R_s[i][0] + sum7[0];
        sum7[1] = p_re2_y_R_s[i][1] + sum7[1];
        sum7[2] = p_re2_y_R_s[i][2] + sum7[2];

        sum8[0] = r_re2_y_R_s[i][0] + sum8[0];
        sum8[1] = r_re2_y_R_s[i][1] + sum8[1];
        sum8[2] = r_re2_y_R_s[i][2] + sum8[2];
    }
    // 求解均值，十周期后可用
    p_re2_z_L_m[0] = sum1[0] / 10;
    p_re2_z_L_m[1] = sum1[1] / 10;
    p_re2_z_L_m[2] = sum1[2] / 10;

    r_re2_z_L_m[0] = sum2[0] / 10;
    r_re2_z_L_m[1] = sum2[1] / 10;
    r_re2_z_L_m[2] = sum2[2] / 10;

    p_re2_y_L_m[0] = sum3[0] / 10;
    p_re2_y_L_m[1] = sum3[1] / 10;
    p_re2_y_L_m[2] = sum3[2] / 10;

    r_re2_y_L_m[0] = sum4[0] / 10;
    r_re2_y_L_m[1] = sum4[1] / 10;
    r_re2_y_L_m[2] = sum4[2] / 10;

    p_re2_z_R_m[0] = sum5[0] / 10;
    p_re2_z_R_m[1] = sum5[1] / 10;
    p_re2_z_R_m[2] = sum5[2] / 10;

    r_re2_z_R_m[0] = sum6[0] / 10;
    r_re2_z_R_m[1] = sum6[1] / 10;
    r_re2_z_R_m[2] = sum6[2] / 10;

    p_re2_y_R_m[0] = sum7[0] / 10;
    p_re2_y_R_m[1] = sum7[1] / 10;
    p_re2_y_R_m[2] = sum7[2] / 10;

    r_re2_y_R_m[0] = sum8[0] / 10;
    r_re2_y_R_m[1] = sum8[1] / 10;
    r_re2_y_R_m[2] = sum8[2] / 10;

    //---------------------------------------------------------------------------------
    //        sigDbgMsg("rowReady : " + QString::number(rowReady));
    RS_tg.clear();
    if (rowReady == 1) {
        QVariantMap rsTg, stateTg;
        rsTg.insert("x_re2_z_L_m", p_re2_z_L_m[0]);
        rsTg.insert("y_re2_z_L_m", p_re2_z_L_m[1]);
        rsTg.insert("z_re2_z_L_m", p_re2_z_L_m[2]);
        rsTg.insert("rx_re2_z_L_m", r_re2_z_L_m[0]);
        rsTg.insert("ry_re2_z_L_m", r_re2_z_L_m[1]);
        rsTg.insert("rz_re2_z_L_m", r_re2_z_L_m[2]);

        rsTg.insert("x_re2_y_L_m", p_re2_y_L_m[0]);
        rsTg.insert("y_re2_y_L_m", p_re2_y_L_m[1]);
        rsTg.insert("z_re2_y_L_m", p_re2_y_L_m[2]);
        rsTg.insert("rx_re2_y_L_m", r_re2_y_L_m[0]);
        rsTg.insert("ry_re2_y_L_m", r_re2_y_L_m[1]);
        rsTg.insert("rz_re2_y_L_m", r_re2_y_L_m[2]);

        rsTg.insert("x_re2_z_R_m", p_re2_z_R_m[0]);
        rsTg.insert("y_re2_z_R_m", p_re2_z_R_m[1]);
        rsTg.insert("z_re2_z_R_m", p_re2_z_R_m[2]);
        rsTg.insert("rx_re2_z_R_m", r_re2_z_R_m[0]);
        rsTg.insert("ry_re2_z_R_m", r_re2_z_R_m[1]);
        rsTg.insert("rz_re2_z_R_m", r_re2_z_R_m[2]);

        rsTg.insert("x_re2_y_R_m", p_re2_y_R_m[0]);
        rsTg.insert("y_re2_y_R_m", p_re2_y_R_m[1]);
        rsTg.insert("z_re2_y_R_m", p_re2_y_R_m[2]);
        rsTg.insert("rx_re2_y_R_m", r_re2_y_R_m[0]);
        rsTg.insert("ry_re2_y_R_m", r_re2_y_R_m[1]);
        rsTg.insert("rz_re2_y_R_m", r_re2_y_R_m[2]);

        stateTg.insert("lineget_flag_L", lineget_flag_L);
        stateTg.insert("lineget_flag_R", lineget_flag_R);

        QVariantMap RS_tgMap;
        RS_tgMap.insert("RS_tg_data", rsTg);
        RS_tgMap.insert("RS_tg_state", stateTg);

        RS_tg << RS_tgMap;

        onTgRsReady();
    }
    //-------------------------------------------------------
    if (DL_state == 1) {
        m_bufPts[coorCnt++] = DL_tg[3];
        m_bufPts[coorCnt++] = DL_tg[4];
        m_bufPts[coorCnt++] = DL_tg[5];
        m_bufPts[coorCnt++] = DL_tg[0];
        m_bufPts[coorCnt++] = DL_tg[1];
        m_bufPts[coorCnt++] = DL_tg[2];
        // sigDbgMsg("DL2APP");
    } else {
        m_bufPts[coorCnt++] = 0.0f;
        m_bufPts[coorCnt++] = 0.0f;
        m_bufPts[coorCnt++] = 0.0f;
        m_bufPts[coorCnt++] = 0.0f;
        m_bufPts[coorCnt++] = 0.0f;
        m_bufPts[coorCnt++] = 0.0f;
        // sigDbgMsg("NO DL2APP");
    }
    //-----------------------------------------------
    m_bufPts[coorCnt++] = ymin_rgbL;
    m_bufPts[coorCnt++] = ymax_rgbL;
    m_bufPts[coorCnt++] = zmin_rgbL;
    m_bufPts[coorCnt++] = zmax_rgbL;
    m_bufPts[coorCnt++] = ytarget_rgbL;
    m_bufPts[coorCnt++] = ztarget_rgbL;

    m_bufPts[coorCnt++] = ymin_rgbR;
    m_bufPts[coorCnt++] = ymax_rgbR;
    m_bufPts[coorCnt++] = zmin_rgbR;
    m_bufPts[coorCnt++] = zmax_rgbR;
    m_bufPts[coorCnt++] = ytarget_rgbR;
    m_bufPts[coorCnt++] = ztarget_rgbR;
    //-----------------------------------------------
    m_bufPts[coorCnt++] = p2z_max_L[0];
    m_bufPts[coorCnt++] = p2z_max_L[1];
    m_bufPts[coorCnt++] = p2z_max_L[2];

    m_bufPts[coorCnt++] = p2z_min_L[0];
    m_bufPts[coorCnt++] = p2z_min_L[1];
    m_bufPts[coorCnt++] = p2z_min_L[2];

    m_bufPts[coorCnt++] = p2y_max_L[0];
    m_bufPts[coorCnt++] = p2y_max_L[1];
    m_bufPts[coorCnt++] = p2y_max_L[2];

    m_bufPts[coorCnt++] = p2y_min_L[0];
    m_bufPts[coorCnt++] = p2y_min_L[1];
    m_bufPts[coorCnt++] = p2y_min_L[2];

    m_bufPts[coorCnt++] = p2z_max_R[0];
    m_bufPts[coorCnt++] = p2z_max_R[1];
    m_bufPts[coorCnt++] = p2z_max_R[2];

    m_bufPts[coorCnt++] = p2z_min_R[0];
    m_bufPts[coorCnt++] = p2z_min_R[1];
    m_bufPts[coorCnt++] = p2z_min_R[2];

    m_bufPts[coorCnt++] = p2y_max_R[0];
    m_bufPts[coorCnt++] = p2y_max_R[1];
    m_bufPts[coorCnt++] = p2y_max_R[2];

    m_bufPts[coorCnt++] = p2y_min_R[0];
    m_bufPts[coorCnt++] = p2y_min_R[1];
    m_bufPts[coorCnt++] = p2y_min_R[2];

    // qDebug() << "coorCnt : " << coorCnt;

    // iamge process

    QByteArray baImgOut;
    QBuffer buf(&baImgOut);
    buf.open(QIODevice::WriteOnly);

    // static quint8* u8arr = new quint8[640 * 480 * 3 * 3];
    // memset(u8arr, 0, 640 * 480 * 3 * 3);

    // sharememory of img lock
    m_smImg->lock();

    QImage imgx((unsigned char*)m_smImg->data(), 640, 480 * 3, 640 * 3,
                QImage::Format_RGB888);

    // memcpy(u8arr, (quint8*)m_smImg->data(), 640 * 480 * 3 * 3);

    // sharememory of img unlock
    m_smImg->unlock();

    // qDebug() << "in camcal (char*)m_smImg->data(): " << ((quint8*)m_smImg->data())[0] << " " << ((quint8*)m_smImg->data())[1] << " " << ((quint8*)m_smImg->data())[2];

    // QImage imgx(u8arr, 640, 480 * 3, 640 * 3, QImage::Format_RGB888);
    imgx.save(&buf, "JPG");

    // QImage imgx(u8arr, 640, 480 * 3, 640 * 3,
    //             QImage::Format_RGB888);

    // test start
    // static int s_nOnceTest = 100;
    // if (s_nOnceTest > 0) {
    //     if (0 == s_nOnceTest % 10) {
    //         qDebug() << "save : "
    //                  << imgx.save(QString("d:/imgs/img3in1_%1.jpeg").arg(s_nOnceTest),
    //                               "JPG");
    //     }
    //     s_nOnceTest -= 1;
    // }
    // test end

    // imgx.save(&buf, "JPG");

    // for (int pI=30; pI<70; ++pI)
    // {
    //     qDebug() << "m_bufPts_0: " << m_bufPts[pI];
    // }

    QByteArray baCloud;
    baCloud.resize(baImgOut.size() + coorCnt * 4);

    memcpy(baCloud.data(), m_bufPts, coorCnt * 4);
    baCloud.insert(coorCnt * 4, baImgOut);

    // qDebug() << "baImgOut size: " << baImgOut.size();

    QVariantMap mapJson;
    mapJson.insert("action", "rsdata");
    mapJson.insert("send", 1);
    mapJson.insert("color", "none");
    mapJson.insert("xmin", xmin);
    mapJson.insert("ymin", ymin);
    mapJson.insert("zmin", zmin);
    mapJson.insert("xmax", xmax);
    mapJson.insert("ymax", ymax);
    mapJson.insert("zmax", zmax);
    mapJson.insert("points", coorCnt / 3);

    QByteArray baCloudSend = JsonUtil::packDataFE(mapJson, baCloud);  // ef->fe

    int nBaCloudSize = baCloudSend.size();
    // baCloud.size();
    m_smBaSend->lock();
    memcpy((char*)m_smBaSend->data(), &nBaCloudSize, 4);
    memcpy((char*)m_smBaSend->data() + 4, baCloudSend.data(), nBaCloudSize);
    m_smBaSend->unlock();

    // qDebug() << "nBaCloudSize: " << nBaCloudSize;
}

void CamCalCenter::setSendRsPosData(const QVariantMap& left,
                                    const QVariantMap& right,
                                    const QString& serial)
{
    s_bSendRsPos = true;
    s_strSerialSendRsPos = serial;
    s_fTargetXL = left.value("x").toFloat();
    s_fTargetYL = left.value("y").toFloat();
    s_fTargetZL = left.value("z").toFloat();
    s_fTargetXwL = left.value("l").toFloat();
    s_fTargetYwL = left.value("w").toFloat();
    s_fTargetZwL = left.value("h").toFloat();

    s_fTargetXR = right.value("x").toFloat();
    s_fTargetYR = right.value("y").toFloat();
    s_fTargetZR = right.value("z").toFloat();
    s_fTargetXwR = right.value("l").toFloat();
    s_fTargetYwR = right.value("w").toFloat();
    s_fTargetZwR = right.value("h").toFloat();

    qDebug() << "recv sendrspos "
             << "left: " << left;
    qDebug() << "recv sendrspos "
             << "right: " << right;
    qDebug() << "\n";
    m_bRunRealsense = true;
}

void CamCalCenter::call_setSendRsPosData(const QVariantMap& left,
                                         const QVariantMap& right,
                                         const QString& serial)
{
    QMetaObject::invokeMethod(this, "setSendRsPosData", Qt::QueuedConnection,
                              Q_ARG(QVariantMap, left),
                              Q_ARG(QVariantMap, right),
                              Q_ARG(QString, serial));
}

void CamCalCenter::call_setDLData(double x,
                                  double y,
                                  double z,
                                  double rx,
                                  double ry,
                                  double rz,
                                  quint16 state)
{
    QMetaObject::invokeMethod(
        this, "setDlData", Qt::QueuedConnection, Q_ARG(double, x),
        Q_ARG(double, y), Q_ARG(double, z), Q_ARG(double, rx),
        Q_ARG(double, ry), Q_ARG(double, rz), Q_ARG(quint16, state));
}

void CamCalCenter::call_setRsState(bool b)
{
    QMetaObject::invokeMethod(this, "setRsState", Qt::QueuedConnection,
                              Q_ARG(bool, b));
}

void CamCalCenter::setDlData(double x,
                             double y,
                             double z,
                             double rx,
                             double ry,
                             double rz,
                             quint16 state)
{
    Vector3f vDL(x, y, z);
    Vector3f vDL0(camParam[3][3], camParam[3][4], camParam[3][5]);
    Vector3f v4 = ROT4 * vDL + vDL0;
    DL_tg[0] = v4(0);
    DL_tg[1] = v4(1);
    DL_tg[2] = v4(2);
    DL_tg[3] = rx + camParam[3][0];
    DL_tg[4] = ry + camParam[3][1];
    DL_tg[5] = rz + camParam[3][2];
    DL_state = state;

    qDebug() << "in setDlData x, y, z: " << x << "  " << y << "  " << z;
}

void CamCalCenter::setRsState(bool b)
{
    m_bRunRealsense = b;
}

void CamCalCenter::call_setGenRParam(int missionNum, const QString& serial)
{
    QMetaObject::invokeMethod(this, "setGenRParam", Qt::QueuedConnection,
                              Q_ARG(int, missionNum), Q_ARG(QString, serial));
}

static QString s_GenRSerial = "";
void CamCalCenter::setGenRParam(int missionNum, const QString& serial)
{
    s_nMissionNum = missionNum;
    s_GenRSerial = serial;
    s_bStartGenR = true;

    qDebug() << "s_bStartGenR = " << s_bStartGenR << " s_nMissionNum = " << s_nMissionNum << "lineget_flag_RSL =" << lineget_flag_RSL << " lineget_flag_RSR =" << lineget_flag_RSR;

    if (s_bStartGenR) {
        qDebug() << "start GenR";
        qDebug() << "DL_TG" << DL_state;
        s_bStartGenR = false;
        //---------------------------------------------------------------------------------
        if ((s_nMissionNum == 0) ||
            (s_nMissionNum == 3) || (s_nMissionNum == 13) || (s_nMissionNum == 8) || (s_nMissionNum == 18) || (s_nMissionNum == 9) || (s_nMissionNum == 19) ||
            (lineget_flag_RSL == 1 && s_nMissionNum == 1 && rowReady == 1) || (lineget_flag_RSR == 1 && s_nMissionNum == 11 && rowReady == 1) ||
            (lineget_flag_RSL == 1 && lineget_flag_RSR == 1 && (s_nMissionNum == 5 || s_nMissionNum == 15 || s_nMissionNum == 25 || s_nMissionNum == 35 || s_nMissionNum == 36 || s_nMissionNum == 46) && rowReady == 1) ||
            (lineget_flag_RSL == 1 && DL_state == 1 && s_nMissionNum == 6 && rowReady == 1) || (lineget_flag_RSR == 1 && DL_state == 1 && s_nMissionNum == 16 && rowReady == 1) ||
            (lineget_flag_RSL == 1 && s_nMissionNum == 4 && rowReady == 1) || (lineget_flag_RSR == 1 && s_nMissionNum == 14 && rowReady == 1) ||
            (lineget_flag_RSL == 1 && s_nMissionNum == 7 && rowReady == 1) || (lineget_flag_RSR == 1 && s_nMissionNum == 17 && rowReady == 1)) {
            tgRS[24] = DL_tg[0];
            tgRS[25] = DL_tg[1];
            tgRS[26] = DL_tg[2];
            tgRS[27] = DL_tg[3];
            tgRS[28] = DL_tg[4];
            tgRS[29] = DL_tg[5];

            TPSGENR(tgRS, s_nMissionNum, TPS_data, toolmass_L_data, cc_L_data, toolmass_R_data, cc_R_data, ftMode_L_data, ftMode_R_data, period_L, period_R, stepNum);
            qDebug() << "s_nMissionNum = " << s_nMissionNum << "stepNum = " << stepNum[0];

            s_listSteps.clear();
            for (int nS = 0; nS < stepNum[0]; ++nS) {
                QVariantMap mapLeft, mapRight;
                mapLeft.insert("x", TPS_data[nS][0] + armParam[0][0]);
                mapLeft.insert("y", TPS_data[nS][1] + armParam[0][1]);
                mapLeft.insert("z", TPS_data[nS][2] + armParam[0][2]);
                mapLeft.insert("rx", TPS_data[nS][3] + armParam[0][3]);
                mapLeft.insert("ry", TPS_data[nS][4] + armParam[0][4]);
                mapLeft.insert("rz", TPS_data[nS][5] + armParam[0][5]);
                mapLeft.insert("elbow", TPS_data[nS][6] + armParam[0][6]);

                mapRight.insert("x", TPS_data[nS][7 + 0] + armParam[1][0]);
                mapRight.insert("y", TPS_data[nS][7 + 1] + armParam[1][1]);
                mapRight.insert("z", TPS_data[nS][7 + 2] + armParam[1][2]);
                mapRight.insert("rx", TPS_data[nS][7 + 3] + armParam[1][3]);
                mapRight.insert("ry", TPS_data[nS][7 + 4] + armParam[1][4]);
                mapRight.insert("rz", TPS_data[nS][7 + 5] + armParam[1][5]);
                mapRight.insert("elbow", TPS_data[nS][7 + 6] + armParam[1][6]);

                // sigDbgMsg("right x = " +  QString::number(TPS_data[nS][7+0]));
                // sigDbgMsg("left x = " +  QString::number(TPS_data[nS][0+0]));

                mapLeft.insert("toolControl", TPS_data[nS][14]);
                mapRight.insert("toolControl", TPS_data[nS][15]);

                mapLeft.insert("tool_com_x", toolmass_L_data[nS][1]);
                mapLeft.insert("tool_com_y", toolmass_L_data[nS][2]);
                mapLeft.insert("tool_com_z", toolmass_L_data[nS][3]);
                mapRight.insert("tool_com_x", toolmass_R_data[nS][1]);
                mapRight.insert("tool_com_y", toolmass_R_data[nS][2]);
                mapRight.insert("tool_com_z", toolmass_R_data[nS][3]);

                QVariantMap mapLeft2, mapRight2;

                mapLeft2.insert("ftMode", ftMode_L_data[nS]);
                mapLeft2.insert("toolmass", toolmass_L_data[nS][0]);
                mapLeft2.insert("periodMode", period_L[nS][0]);
                mapLeft2.insert("periodValue", period_L[nS][1]);

                mapLeft2.insert("kx", cc_L_data[nS][0]);
                mapLeft2.insert("ky", cc_L_data[nS][1]);
                mapLeft2.insert("kz", cc_L_data[nS][2]);
                mapLeft2.insert("krx", cc_L_data[nS][3]);
                mapLeft2.insert("kry", cc_L_data[nS][4]);
                mapLeft2.insert("krz", cc_L_data[nS][5]);
                mapLeft2.insert("cx", cc_L_data[nS][6]);
                mapLeft2.insert("cy", cc_L_data[nS][7]);
                mapLeft2.insert("cz", cc_L_data[nS][8]);
                mapLeft2.insert("crx", cc_L_data[nS][9]);
                mapLeft2.insert("cry", cc_L_data[nS][10]);
                mapLeft2.insert("crz", cc_L_data[nS][11]);

                mapRight2.insert("ftMode", ftMode_R_data[nS]);
                mapRight2.insert("toolmass", toolmass_R_data[nS][0]);
                mapRight2.insert("periodMode", period_R[nS][0]);
                mapRight2.insert("periodValue", period_R[nS][1]);

                mapRight2.insert("kx", cc_R_data[nS][0]);
                mapRight2.insert("ky", cc_R_data[nS][1]);
                mapRight2.insert("kz", cc_R_data[nS][2]);
                mapRight2.insert("krx", cc_R_data[nS][3]);
                mapRight2.insert("kry", cc_R_data[nS][4]);
                mapRight2.insert("krz", cc_R_data[nS][5]);
                mapRight2.insert("cx", cc_R_data[nS][6]);
                mapRight2.insert("cy", cc_R_data[nS][7]);
                mapRight2.insert("cz", cc_R_data[nS][8]);
                mapRight2.insert("crx", cc_R_data[nS][9]);
                mapRight2.insert("cry", cc_R_data[nS][10]);
                mapRight2.insert("crz", cc_R_data[nS][11]);

                QVariantMap mapOne;
                mapOne.insert("left", mapLeft);
                mapOne.insert("right", mapRight);

                mapOne.insert("leftParm", mapLeft2);
                mapOne.insert("rightParm", mapRight2);

                s_listSteps << mapOne;
            }

            qDebug() << "s_listSteps size: " << QString::number(s_listSteps.size());
            qDebug() << "s_nMissionNum: " << QString::number(s_nMissionNum);
            onGenRReady(0, stepNum[0], s_nMissionNum, s_listSteps);
        }
    }
}

void CamCalCenter::onGenRReady(int curStep, int totalStep, int operate, const QVariantList& steps)
{
    QVariantMap mapJson;
    mapJson.insert("action", "operateControl");
    mapJson.insert("result", "0000");
    mapJson.insert("finish", 0);
    mapJson.insert("serial", s_GenRSerial);
    mapJson.insert("currStep", curStep);
    mapJson.insert("totalStep", totalStep);
    mapJson.insert("operate", operate);
    //    mapJson.insert("steps", steps);

    QVariantList listNewSteps;
    for (int i = 0; i < s_listSteps.size(); ++i) {
        QVariantMap mapMoveToLeft;
        mapMoveToLeft.insert("moveTo", s_listSteps.at(i).toMap().value("left"));

        QVariantMap mapMoveToRight;
        mapMoveToRight.insert("moveTo", s_listSteps.at(i).toMap().value("right"));

        QVariantMap mapOne;
        mapOne.insert("left", mapMoveToLeft);
        mapOne.insert("right", mapMoveToRight);
        listNewSteps << mapOne;
    }
    mapJson.insert("steps", listNewSteps);

    QByteArray ba = JsonUtil::packData(mapJson, QByteArray());

    m_robotCmdNode->pubMapText("onGenRReady", ba.size(), ba);

    // while (nullptr!=s_socket50055 && s_socket50055->isValid() && nWrite<ba.size())
    // {
    //     nWrite += s_socket50055->write(ba.mid(nWrite));
    //     qDebug() << __FUNCTION__ << "s_socket50055 total: " << ba.size() << "  ,write: " << nWrite;
    // }

    // qDebug() << "calculated steps: " << steps;
    // qDebug() << "worker onGenRReady write:   " << nWrite;
    // qDebug() << "\n";
}

void CamCalCenter::call_trigger(int leftT, int rightT)
{
    QMetaObject::invokeMethod(this, "trigger", Qt::QueuedConnection,
                              Q_ARG(int, leftT), Q_ARG(int, rightT));
}

void CamCalCenter::trigger(int leftT, int rightT)
{
    if (s_listSteps.size() > 0) {
        QVariantMap mapCurStep = s_listSteps.at(s_nCurStepIndex).toMap();

        QVariantMap mapLeft = mapCurStep.value("left").toMap();
        QVariantMap mapRight = mapCurStep.value("right").toMap();

        qDebug() << __FUNCTION__ << "mapLeft: " << JsonUtil::map2jsonStr(mapLeft);
        qDebug() << __FUNCTION__ << "mapRight: " << JsonUtil::map2jsonStr(mapRight);

        quint8 nLeftToolControl = int(roundf(mapLeft.value("toolControl").toFloat()));
        quint8 nRightToolControl = int(roundf(mapRight.value("toolControl").toFloat()));

        QVariantMap mapLeftParm = mapCurStep.value("leftParm").toMap();
        QVariantMap mapRightParm = mapCurStep.value("rightParm").toMap();

        quint16 ftMode_L = int(roundf(mapLeftParm.value("ftMode").toFloat()));
        quint16 ftMode_R = int(roundf(mapRightParm.value("ftMode").toFloat()));

        quint16 periodMode_L = int(roundf(mapLeftParm.value("periodMode").toFloat()));
        quint16 periodMode_R = int(roundf(mapRightParm.value("periodMode").toFloat()));

        // Tcp::instance()->setLeftFTMode(ftMode_L);
        // Tcp::instance()->setRightFTMode(ftMode_R);
        // Tcp::instance()->setLeftSecMode(periodMode_L);
        // Tcp::instance()->setRightSecMode(periodMode_R);

        qDebug() << __FUNCTION__ << "nLeftToolControl: " << nLeftToolControl << "  nRightToolControl:" << nRightToolControl << " leftT:" << leftT << "  rightT:" << rightT;

        QVariantMap mapMode;
        mapMode.insert("ftMode_L", ftMode_L);
        mapMode.insert("ftMode_R", ftMode_R);
        mapMode.insert("periodMode_L", periodMode_L);
        mapMode.insert("periodMode_R", periodMode_R);
        m_robotCmdNode->sendCmd("part_trigger",
                                JsonUtil::map2jsonStr(mapMode).toStdString());

        if (leftT > 0 || rightT > 0) {
            if (nLeftToolControl < 6) {
                // s_instantDoCtrl->Write(0, nLeftToolControl);
                m_robotCmdNode->clawControl(0, nLeftToolControl);
            } else {
                // s_instantDoCtrl->Write(0, 5);
                m_robotCmdNode->clawControl(0, 5);
            }

            if (nRightToolControl < 6) {
                // s_instantDoCtrl->Write(1, nRightToolControl);
                m_robotCmdNode->clawControl(1, nRightToolControl);
            } else {
                // s_instantDoCtrl->Write(1, 5);
                m_robotCmdNode->clawControl(1, 5);
            }

            if (nRightToolControl == 6)  // suojin
            {
                m_robotCmdNode->sendCmd("pinch", "spin45");
            } else if (nRightToolControl == 7)  // songkai
            {
                m_robotCmdNode->sendCmd("pinch", "songkai");
            } else if (nRightToolControl == 8)  // suojin
            {
                m_robotCmdNode->sendCmd("pinch", "180x9");
            } else if (nRightToolControl == 9)  // suojin
            {
                m_robotCmdNode->sendCmd("pinch", "spinBack45");
            } else if (nRightToolControl == 10)  // suojin
            {
                m_robotCmdNode->sendCmd("pinch", "spin45");
            }

            if (nRightToolControl == 30)  // suojin
            {
                m_robotCmdNode->sendCmd("pinch", "spin45");
            } else if (nRightToolControl == 31)  // songkai
            {
                m_robotCmdNode->sendCmd("pinch", "songkai");
            } else if (nRightToolControl == 32)  // suojin
            {
                m_robotCmdNode->sendCmd("pinch", "180x9");
            } else if (nRightToolControl == 33)  // suojin
            {
                m_robotCmdNode->sendCmd("pinch", "spinBack45");
            } else if (nRightToolControl == 34)  // suojin
            {
                m_robotCmdNode->sendCmd("pinch", "stop");
            } else if (nRightToolControl == 35)  // suojin
            {
                m_robotCmdNode->sendCmd("pinch", "resetPos");
            }

            // for serial peeler
            // reset
            else if (nRightToolControl == 20) {
                m_robotCmdNode->sendCmd("peel", "reset");
            }
            // pinch
            else if (nRightToolControl == 21) {
                m_robotCmdNode->sendCmd("peel", "pinch");
            }
            // loose
            else if (nRightToolControl == 22) {
                m_robotCmdNode->sendCmd("peel", "loose");
            }
            // stop
            else if (nRightToolControl == 23) {
                m_robotCmdNode->sendCmd("peel", "stop");
            }
            // startPeel
            else if (nRightToolControl == 24) {
                m_robotCmdNode->sendCmd("peel", "startPeel");
            }
            // stopPeel
            else if (nRightToolControl == 25) {
                m_robotCmdNode->sendCmd("peel", "stopPeel");
            }

            if (nLeftToolControl == 6)  // suojin
            {
                m_robotCmdNode->sendCmd("pinch", "spin45");
            } else if (nLeftToolControl == 7)  // songkai
            {
                m_robotCmdNode->sendCmd("pinch", "songkai");
            } else if (nLeftToolControl == 8)  // suojin
            {
                m_robotCmdNode->sendCmd("pinch", "180x9");
            } else if (nLeftToolControl == 9)  // suojin
            {
                m_robotCmdNode->sendCmd("pinch", "spinBack45");
            } else if (nLeftToolControl == 10)  // suojin
            {
                m_robotCmdNode->sendCmd("pinch", "spin45");
            }

            if (nLeftToolControl == 30)  // suojin
            {
                m_robotCmdNode->sendCmd("pinch", "spin45");
            } else if (nLeftToolControl == 31)  // songkai
            {
                m_robotCmdNode->sendCmd("pinch", "songkai");
            } else if (nLeftToolControl == 32)  // suojin
            {
                m_robotCmdNode->sendCmd("pinch", "180x9");
            } else if (nLeftToolControl == 33)  // suojin
            {
                m_robotCmdNode->sendCmd("pinch", "spinBack45");
            } else if (nLeftToolControl == 34)  // suojin
            {
                m_robotCmdNode->sendCmd("pinch", "stop");
            } else if (nLeftToolControl == 35)  // suojin
            {
                m_robotCmdNode->sendCmd("pinch", "resetPos");
            }

            // for serial peeler
            // reset
            else if (nLeftToolControl == 20) {
                m_robotCmdNode->sendCmd("peel", "reset");
            }
            // pinch
            else if (nLeftToolControl == 21) {
                m_robotCmdNode->sendCmd("peel", "pinch");
            }
            // loose
            else if (nLeftToolControl == 22) {
                m_robotCmdNode->sendCmd("peel", "loose");
            }
            // stop
            else if (nLeftToolControl == 23) {
                m_robotCmdNode->sendCmd("peel", "stop");
            }
            // startPeel
            else if (nLeftToolControl == 24) {
                m_robotCmdNode->sendCmd("peel", "startPeel");
            }
            // stopPeel
            else if (nLeftToolControl == 25) {
                m_robotCmdNode->sendCmd("peel", "stopPeel");
            }
        }
    }
}

void CamCalCenter::call_operate(int nExecStep, const QString& strSerial)
{
    QMetaObject::invokeMethod(this, "operate", Qt::QueuedConnection,
                              Q_ARG(int, nExecStep), Q_ARG(QString, strSerial));
}

void CamCalCenter::operate(int nExecStep, const QString& strSerial)
{
    qDebug() << __FUNCTION__ << "  exestep: " << nExecStep;

    if (nExecStep > s_listSteps.size() || nExecStep <= 0) {
        return;
    }

    s_nCurStepIndex = nExecStep - 1;

    QVariantMap mapCurStep = s_listSteps.at(s_nCurStepIndex).toMap();
    s_GenRSerial = strSerial;

    QVariantMap mapLeft = mapCurStep.value("left").toMap();
    QVariantMap mapRight = mapCurStep.value("right").toMap();

    QVariantMap mapLeftParm = mapCurStep.value("leftParm").toMap();
    QVariantMap mapRightParm = mapCurStep.value("rightParm").toMap();

    qDebug() << "current step in operate left: " << mapLeft;
    qDebug() << "current step in operate right: " << mapRight;

    // uncomment to control robot
    QVariantMap leftCmd;
    leftCmd.insert("x2", mapLeft.value("x"));
    leftCmd.insert("y2", mapLeft.value("y"));
    leftCmd.insert("z2", mapLeft.value("z"));
    leftCmd.insert("rx2", mapLeft.value("rx"));
    leftCmd.insert("ry2", mapLeft.value("ry"));
    leftCmd.insert("rz2", mapLeft.value("rz"));
    leftCmd.insert("elbow2", mapLeft.value("elbow"));
    leftCmd.insert("toolComX", mapLeft.value("tool_com_x"));
    leftCmd.insert("toolComY", mapLeft.value("tool_com_y"));
    leftCmd.insert("toolComZ", mapLeft.value("tool_com_z"));

    leftCmd.insert("kx", mapLeftParm.value("kx"));
    leftCmd.insert("ky", mapLeftParm.value("ky"));
    leftCmd.insert("kz", mapLeftParm.value("kz"));
    leftCmd.insert("krx", mapLeftParm.value("krx"));
    leftCmd.insert("kry", mapLeftParm.value("kry"));
    leftCmd.insert("krz", mapLeftParm.value("krz"));
    leftCmd.insert("cx", mapLeftParm.value("cx"));
    leftCmd.insert("cy", mapLeftParm.value("cy"));
    leftCmd.insert("cz", mapLeftParm.value("cz"));
    leftCmd.insert("crx", mapLeftParm.value("crx"));
    leftCmd.insert("cry", mapLeftParm.value("cry"));
    leftCmd.insert("crz", mapLeftParm.value("crz"));

    leftCmd.insert("toolWeight", mapLeftParm.value("toolmass"));
    leftCmd.insert("periodValue", mapLeftParm.value("periodValue"));

    leftCmd.insert("trigger", 0);

    QVariantMap rightCmd;
    rightCmd.insert("x2", mapRight.value("x"));
    rightCmd.insert("y2", mapRight.value("y"));
    rightCmd.insert("z2", mapRight.value("z"));
    rightCmd.insert("rx2", mapRight.value("rx"));
    rightCmd.insert("ry2", mapRight.value("ry"));
    rightCmd.insert("rz2", mapRight.value("rz"));
    rightCmd.insert("elbow2", mapRight.value("elbow"));
    rightCmd.insert("toolComX", mapRight.value("tool_com_x"));
    rightCmd.insert("toolComY", mapRight.value("tool_com_y"));
    rightCmd.insert("toolComZ", mapRight.value("tool_com_z"));

    rightCmd.insert("kx", mapRightParm.value("kx"));
    rightCmd.insert("ky", mapRightParm.value("ky"));
    rightCmd.insert("kz", mapRightParm.value("kz"));
    rightCmd.insert("krx", mapRightParm.value("krx"));
    rightCmd.insert("kry", mapRightParm.value("kry"));
    rightCmd.insert("krz", mapRightParm.value("krz"));
    rightCmd.insert("cx", mapRightParm.value("cx"));
    rightCmd.insert("cy", mapRightParm.value("cy"));
    rightCmd.insert("cz", mapRightParm.value("cz"));
    rightCmd.insert("crx", mapRightParm.value("crx"));
    rightCmd.insert("cry", mapRightParm.value("cry"));
    rightCmd.insert("crz", mapRightParm.value("crz"));

    rightCmd.insert("toolWeight", mapRightParm.value("toolmass"));
    rightCmd.insert("periodValue", mapRightParm.value("periodValue"));

    rightCmd.insert("trigger", 0);

    // Tcp::instance()->writeDuoArmDirect(leftCmd, rightCmd);
    QVariantMap mapArmCmd;
    mapArmCmd.insert("left", leftCmd);
    mapArmCmd.insert("right", rightCmd);
    m_robotCmdNode->sendCmd(
        "writeDuo_camcal", JsonUtil::map2jsonStr(mapArmCmd).toStdString());

    QVariantMap mapJson;
    mapJson.insert("action", "operateControl");
    mapJson.insert("serial", s_GenRSerial);
    mapJson.insert("result", "0000");
    mapJson.insert("finish", nExecStep == s_listSteps.size() ? 1 : 0);
    mapJson.insert("currStep", nExecStep);
    mapJson.insert("totalStep", s_listSteps.size());
    mapJson.insert("operate", s_nMissionNum);

    QVariantList listNewSteps;
    for (int i = 0; i < s_listSteps.size(); ++i) {
        QVariantMap mapMoveToLeft;
        mapMoveToLeft.insert("moveTo", s_listSteps.at(i).toMap().value("left"));

        QVariantMap mapMoveToRight;
        mapMoveToRight.insert("moveTo", s_listSteps.at(i).toMap().value("right"));

        QVariantMap mapOne;
        mapOne.insert("left", mapMoveToLeft);
        mapOne.insert("right", mapMoveToRight);
        listNewSteps << mapOne;
    }

    mapJson.insert("steps", listNewSteps);

    QByteArray ba = JsonUtil::packData(mapJson, QByteArray());

    m_robotCmdNode->pubMapText("operate", ba.size(), ba);

    qint64 nWrite = 0;
    qDebug() << "operateControl operate: " << s_nMissionNum
             << " currStep:" << nExecStep;
}
